#pragma once

#include "car_init.hpp"
#include <thread>
#include <chrono>
#include <mutex>
#include <cmath>
#include <atomic>
#include "yj_time.hpp"
#include <queue>

typedef enum
{
    waiting_mode_car_x = 0,
    waiting_mode_car_y,
    waiting_mode_car_yaw,
    waiting_mode_claw_y,
    waiting_mode_car_x_y_yaw,
} waiting_mode_en;

typedef enum
{
    trace_mode_claw_y = 0,
    trace_mode_car_y
} trace_mode_en;

yj_return car_control_init(void);

void set_car_yaw_angle(double yaw);
void set_car_yaw_speed_limit(double max_speed, double min_speed);
void set_car_x_mm(double x);
void set_car_x_speed_limit(double max_speed, double min_speed);
void set_car_y_mm(double y);
void set_car_y_speed_limit(double max_speed, double min_speed);
void set_car_move(double x, double y, double yaw);

double get_car_yaw_angle(void);
double get_car_x_mm(void);
double get_car_y_mm(void);

void yaw_sensor_read_thread_entry();
void xy_sensor_read_thread_entry();
void xy_sensorless_read_thread_entry();
void car_yaw_control_thread_entry();
void car_x_control_thread_entry();
void car_y_control_thread_entry();
void car_all_control_thread_entry();

void set_claw_y_mm(double y);
void set_claw_y_speed_limit(double max_speed, double min_speed);
void start_trace_color(trace_mode_en mode, camera::camera_color_en color, double is_ok_threshold);
// void enable_claw_control(void);
// void disable_claw_control(void);
void car_y_claw_control_thread_entry();
yj_return waiting_for_move_finish(waiting_mode_en mode, uint64_t timeout_ms, double death_zone);

extern camera *cam;
extern pid_pos *x_pid_out;
extern pid_pos *x_pid_in;
extern pid_double_ring *x_pid;

yj_return start_trace_color_with_claw(camera::camera_color_en color, uint64_t timeout_ms, double death_zone, bool if_speed_death_zone = false);
yj_return get_claw_trace_data(double &angle, double &claw_y);